#include <iostream>
#include <vector>
#include <Eigen/Core>
#include <Utils.h>
#include <ceres/ceres.h>
#include <sophus/so3.hpp>
using namespace std;
using namespace Eigen;
using namespace lyxutils::abbreviations;

void print_usage(){
    cout<<"usage:align_planes ref_planes.txt align_planes.txt [-o result.txt]"<<endl;
    cout<<"explain:esitimate the best similarity transformation matrix between"<<endl;
    cout<<"        the given matched plane pairs.columns in ref_planes.txt and"<<endl;
    cout<<"        align_planes.txt:center point,xAXis,yAXis,zAxis=>12 columns"<<endl;
    cout<<"arguments:"<<endl;
    cout<<"  -h         print this help message"<<endl;
    cout<<"  -o=result.txt   set output file name(default is result.txt)"<<endl;
    cout<<"  -a=degree  degree of angle equivalent to 1m distance(default is 1,"<<endl;
    cout<<"             meaning angle error of one degree is equivalent to 1m"<<endl;
    cout<<"             distance error),set sensitivity to angle and distance"<<endl;
}

struct Plane{
    Vec3d center;
    Vec3d norm;
    Plane(){}
    Plane(const Plane &another){//clone constructor
        center=another.center;
        norm=another.norm;
    }
    static void fromFile(string filename,vector<Plane> &planes){
        xio::read_csv_customize(filename,[&](const vector<float> &row){
            if(row.size()>=6){
                Plane p;
                for(int i=0;i<3;++i){
                    p.center[i]=row[i];
                    p.norm[i]=row[i+3];
                }
                planes.push_back(p);
            }
            else{
                throw invalid_argument("input file "+filename+" damaged:not enough cols");
            }
        });
    }
};

struct PlaneErrorCost{
    PlaneErrorCost(Plane src,Plane dest,double degree): align(src), ref(dest), _radian(degree * M_PI / 180){}
    //残差计算
    bool operator()(const double *const phi_t_alpha, double *residuals)const{
        //模型参数phi_t_alpha,有7个，前三个是李代数,后三个是平移量,最后一个是缩放
        Vector3d vec_so3(phi_t_alpha[0], phi_t_alpha[1], phi_t_alpha[2]);
        Matrix3d R=Sophus::SO3d::exp(vec_so3).matrix();
        Vec3d row1(R(0,0),R(0,1),R(0,2));
        Vec3d row2(R(1,0),R(1,1),R(1,2));
        Vec3d row3(R(2,0),R(2,1),R(2,2));
        Vec3d t(phi_t_alpha[3],phi_t_alpha[4],phi_t_alpha[5]);
        double alpha=phi_t_alpha[6];
        Vec3d X_trans=alpha*Vec3d(row1.dot(align.center),
                row2.dot(align.center),row3.dot(align.center))+t;
        Vec3d norm=Vec3d(row1.dot(align.norm),row2.dot(align.norm),row3.dot(align.norm));
        residuals[0]=ref.norm.dot(X_trans-ref.center);
        residuals[1]= ceres::acos(ref.norm.dot(norm))/ _radian;
        return true;
    }

    const Plane align,ref;
    const double _radian;
};

int main(int argc,char **argv) {
    xio::CLParser parser;
    try{
        parser.parse(argc,argv);
    }catch(invalid_argument ia){
        cout<<"invalid argument:"<<ia.what()<<endl;
        print_usage();
        return 1;
    }
    if(argc<3||parser.hasOption("h")){
        print_usage();
        return 1;
    }
    vector<string> parameters=parser.getParameters();
    for(int i=0;i<2;++i){
        if(!xos::file(xos::abspath(parameters[i]))){
            cout<<"file:"<<parameters[i]<<" not exist!"<<endl;
            return 1;
        }//test file existence
    }
    vector<Plane> align_planes,ref_planes;
    try{
        Plane::fromFile(parameters[0],ref_planes);
        Plane::fromFile(parameters[1],align_planes);
    }catch(invalid_argument ia){
        cout<<ia.what()<<endl;
        return 1;
    }
    if(align_planes.size()!=ref_planes.size()){
        cout<<"number of planes in two files not coincide!";
        cout<<",the redundant planes will be neglected"<<endl;
    }
    string outputFile="result.txt";
    if(parser.hasOption("o")&&parser.getOptionArg("o")!=""){
        outputFile=parser.getOptionArg("o");
        if(xos::file(outputFile)){
            cout<<"output file:"<<outputFile<<" already exist,change to default:";
            outputFile="result.txt";
            cout<<outputFile<<endl;
        }
    }
    double degree=1.0;
    if(parser.hasOption("a")){
        try{
            degree=stod(parser.getOptionArg("a"));
        }
        catch(...){
            cout<<"exception occurred while setting degree option -a="<<parser.getOptionArg("a")<<endl;
            cout<<"the default value will be used:"<<degree<<endl;
        }
    }
    //do some real work
    //构建问题
    double phi_t_alpha[7]={0};
    cout<<"初始参数:";
    for(int i=0;i<7;++i)cout<<phi_t_alpha[i]<<",";
    cout<<endl;
    ceres::Problem problem;
    int N=align_planes.size()<ref_planes.size()?align_planes.size():ref_planes.size();
    for(int i=0;i<N;++i){
        problem.AddResidualBlock(//自动求导,模板参数依次为成本函数类型,残差个数,参数个数
                new ceres::NumericDiffCostFunction<PlaneErrorCost,ceres::RIDDERS,2,7>(
                        new PlaneErrorCost(align_planes[i],ref_planes[i],degree)
                ),
                nullptr,
                phi_t_alpha
        );
    }
    //配置求解器
    ceres::Solver::Options options;
    options.linear_solver_type=ceres::DENSE_NORMAL_CHOLESKY;
    options.minimizer_progress_to_stdout=true;//输出到cout
    ceres::Solver::Summary summary;
    ceres::Solve(options,&problem,&summary);
    //输出结果
    cout<<summary.BriefReport()<<endl;
    Matrix3d R=Sophus::SO3d::exp(Vector3d(phi_t_alpha[0],phi_t_alpha[1],phi_t_alpha[2])).matrix();
    Vector3d t(phi_t_alpha[3],phi_t_alpha[4],phi_t_alpha[5]);
    double alpha=phi_t_alpha[6];
    cout<<"result:"<<endl;
    double RMS=sqrt(summary.final_cost/(N-1));
    cout<<"RMS="<<RMS<<endl;
    cout<<"alpha="<<alpha<<endl;
    cout<<"t=("<<t(0)<<","<<t(1)<<","<<t(2)<<")"<<endl;
    cout<<"so3=("<<phi_t_alpha[0]<<","<<phi_t_alpha[1]<<","<<phi_t_alpha[2]<<")"<<endl;
    cout<<"R="<<endl;
    cout<<R<<endl;
    R=alpha*R;
    cout<<"T="<<endl;
    Matrix4d T;
    T.topLeftCorner(3,3)=R;
    T.row(3)=Matrix<double,1,4>(0,0,0,1);
    T.topRightCorner(3,1)=t;
    cout<<T<<endl;
    ofstream ofs(outputFile);
    ofs<<setprecision(8);
    for(int i=0;i<T.rows();++i){
        ofs<<T(i,0);
        for(int j=1;j<T.cols();++j){
            ofs<<","<<T(i,j);
        }
        ofs<<endl;
    }
    ofs.flush();
    if(ofs.is_open())ofs.close();
    cout<<"result saved to file:"<<outputFile<<endl;
    return 0;
}
